#include "system_app.h"
#include <stdio.h>
/**
 ****************************************************************************************************
 *串口：
 *      串口1：bearpi
 *              TX1---->PA9
 *              RX1---->PA10
 *      串口2：4g
 *              TX2---->PA2
 *              RX2---->PA3
 *      串口3：蓝牙
 *              TX3---->PB10
 *              RX3---->PB11
 *      串口4：test
 *              TX4---->PC10
 *              RX4---->PC11
 *测试灯：
 *      LED ---->PA4
 *传感器：
 *      传感器1---->PA6        
 *      传感器2---->PA7            
 *      传感器3---->PB0       
 *      传感器4---->PB1     
 *      传感器5---->PC0       
 *      传感器6---->PC1       
 *      传感器7---->PC2      
 *      传感器8---->PC3      
 * 
 *      DS18B20传感器---->PA5
 *      超声波传感器：
 *           TRIG---->PE0
 *           ECHO---->PE1
 *编码器：
 *      编码器1：timer3 
 *              L1---->PC6
 *              L2---->PC7
 *      编码器2：timer4
 *              R1---->PD12
 *              R2---->PD13 
 *电机：timer1 
 *      左电机:
 *              PWM1---->PE9
 *              AIN1---->PB12       
 *              AIN2---->PB13       
 *      右电机：
 *              PWM3---->PE13
 *              BIN1---->PB14
 *              BIN2---->PB15
 *      水泵电机：
 *              PWM2---->PE11
 *              CIN1---->PD8
 *              CIN2---->PD9
 *      绕线电机：
 *              PWM4---->PE14
 *              DIN1---->PD10
 *              DIN2---->PD11
 * 
 *      风动电机左：
 *              PWM5---->PC6
 *              AIN1---->PB12       
 *              AIN2---->PB13  
 *      风动电机右：
 *              PWM6---->PC8
 *              BIN1---->PB14
 *              BIN2---->PB15
 *      风动转向电机左：
 *              PWM7---->PC7
 *              CIN1---->PD8
 *              CIN2---->PD9
 *      风动转向电机右：
 *              PWM8---->PC9
 *              DIN1---->PD10
 *              DIN2---->PD11
 * 
 *MPU6050:I2C1
 *      SDA---->PB7
 *      SCL---->PB6
 *      INT---->PB5
 *NRF24L01:SPI3 timer8 
 *      MISO---->PB4
 *      MOSI---->PC12
 *      SCK ---->PB3
 *      CE  ---->PD7
 *      CSN ---->PD6
 *      IRQ ---->PD5
 *舵机：timer2
 *      pwm1---->PA0
 *      PWM2---->PA1
 *ADC数据采集：timer8
 *
 * 
 ****************************************************************************************************
 */


/**
 ****************************************************************************************************
 * DONE 变量
 ****************************************************************************************************
 */

 //定时器8状态位
uint8_t TIMER8_FLAG;
//ADC数据处理状态位
uint8_t ADC_FLAG;
//系统状态位
uint8_t SYSTEM_FLAG;
//NRF数据处理状态位
uint8_t NRF24_FLAG;
//GPS数据处理状态位
uint8_t GNSS_FLAG;

uint16_t ADC_Value[8]={0};
//char ADC_Vlauec[8][12];
char ADC_Vlauec[20];

#define SHOW_HUART Test_huart

ShipMoveState move_state = {0};


/**
 ****************************************************************************************************
 * 系统函数
 ****************************************************************************************************
 */

/**
 * @name 测试灯函数
 * @brief 控制测试灯的亮灭
 * @param time：时间值，0为灭灯，1为亮灯，其他值/2为闪灯时间间隔
 * @retval 无
 */
void system_Test_LED(uint16_t time)
{
    if(time==0)
    {
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
    }
    else if(time==1)
    {
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);
    }
    else
    {
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_SET);
        HAL_Delay(time/2);
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
        HAL_Delay(time/2);
    }
}

/**
 * @name 系统初始化函数
 * @brief 对系统全部外设的初始化（DMA、串口、舵机、传感器、电机、PID、NRF24L01、4g-GNSS、定时器启动、调试信息
 */
void system_Init(void)
{
    uart_Init();
    USART_printf(SHOW_HUART,"SERVO INIT START\r\n");   
    Servo_Init();
    USART_printf(SHOW_HUART,"SERVO INIT END\r\n");   
    USART_printf(SHOW_HUART,"SENSOR INIT START\r\n"); 
    sensor_Init();
    USART_printf(SHOW_HUART,"SENSOR INIT END\r\n");  
    USART_printf(SHOW_HUART,"MOTOR INIT START\r\n");    
    Motor_Init();
    USART_printf(SHOW_HUART,"MOTOR INIT END\r\n");   
    PID_Init(&speed_pid,10.0,0.0,0.5,10.0);
    PID_Init(&position_pid,10.0,0.0,0.5,10.0);
    PID_Init(&angle_pid,10.0,0.0,0.5,10.0);
    USART_printf(SHOW_HUART,"NRF INIT START\r\n"); 
    NRF24_Init();
    USART_printf(SHOW_HUART,"NRF INIT END\r\n"); 
    // G4_Init(G4_WIFI_MQTT);
    USART_printf(SHOW_HUART,"4G INIT START\r\n");    
    G4_GNSS_Start(); 
    USART_printf(SHOW_HUART,"4G INIT END\r\n");    

    HAL_TIM_Base_Start_IT(&htim5);
    HAL_TIM_Base_Start_IT(&htim8);
    HAL_TIM_Base_Start_IT(&htim9);

    USART_printf(SHOW_HUART,"SYSTEM INIT OK\r\n");
}

/**
 * @name 系统运行函数
 * @brief 系统运行逻辑：ADC数据处理（单通道轮询），无线遥控接收数据处理
 */
void system_run(void)	
{
    if(SYSTEM_FLAG == 0)
    {
        USART_printf(SHOW_HUART,"SYSTEM RUN START\r\n");

        move_started = 0;
        
        SYSTEM_FLAG = 1;
    }

    system_Test_LED(1);

    if(ADC_FLAG == 1)
	{
        USART_printf(SHOW_HUART,"ADC READ START\r\n");
        ADC_DATA_READ(&hadc1,ADC1_Channel_Number,ADC_Values1);
        USART_printf(SHOW_HUART,"ADC READ END\r\n");
        ADC_FLAG++;
	}
    else if(ADC_FLAG == 2)
	{
        USART_printf(SHOW_HUART,"ADC READ START\r\n");
        ADC_DATA_READ(&hadc1,ADC2_Channel_Number,ADC_Values2);
        USART_printf(SHOW_HUART,"ADC READ END\r\n");
        USART_printf(SHOW_HUART,"GET TEMPERATURE START\r\n");
        sensor_GetTemperature();
        USART_printf(SHOW_HUART,"GET TEMPERATURE END\r\n");
        USART_printf(SHOW_HUART,"GET DISTANCE START\r\n");
        // sensor_Distance();
        USART_printf(SHOW_HUART,"GET DISTANCE END\r\n");
        ADC_FLAG++;
	}
    else if(ADC_FLAG == 3)
    {
        USART_printf(SHOW_HUART,"DATA Dispose START\r\n");
        // if(ADC_DATA_Dispose_status)
        ADC_DATA_Dispose(bear_huart);
        // else ADC_DATA_Dispose_random(bear_huart);
        // USART_printf(SHOW_HUART,"                                         adc_raw: %.2f\r\n",ADC_Data.adc_raw[3]);
        // USART_printf(SHOW_HUART,"                                         PH: %.2f\r\n",ADC_Data.adc_map[0]);
        USART_printf(SHOW_HUART,"DATA Dispose END\r\n");
        ADC_FLAG++;
    }

    if(NRF24_FLAG)
    {
        RX_Dispose(RF2G4_Receive_Data); 
        NRF24_FLAG = 0;
    }

    if(GNSS_FLAG)
    {
        G4_GNSSDATA_Dispose(bear_huart);
        GNSS_FLAG = 0;
    }
    
    //自动巡航
    // if (move_started == 1) 
    // {
    //     // 启动向前移动1米
    //     SHIP_MOVE_DISTANCE(100, AUTO_MOVE_DIR, 1.0, &move_state);
    //     move_started = 2;
    // }
    
    // // 检查运动是否完成 
    // if (SHIP_MOVE_DISTANCE(0, AUTO_MOVE_DIR, 0, &move_state)) 
    // {
    //     // 运动已完成，可以执行下一步操作 
    //     move_started = 0;
    // }

    //半自动巡航
    if(move_started && !move_starte)
    {
        if(!AUTO_MOVE_TURN && !AUTO_MOVE_TIMER)
        {
            SHIP_RUN(100,100, AUTO_MOVE_DIR);
            AUTO_MOVE_TIMER = 1;
        }  
        else 
        {
            if(AUTO_MOVE_TURN_L == 1)
            {
                SHIP_RUN_LM(100,MOTOR_reversal);
                SHIP_RUN_RM(100,MOTOR_foreward);
            }
            else if(AUTO_MOVE_TURN_L == 2)
            {
                SHIP_RUN_LM(100,MOTOR_foreward);
                SHIP_RUN_RM(100,MOTOR_reversal);
            }
            // SHIP_RUN(70,90, AUTO_MOVE_DIR);
            AUTO_MOVE_DIR = SHIP_FRONT;
        }
    }
    else
    {
        if(motor_start_flag == 1 && water_measure_flag == 1)
        {
            AUTO_MOVE_TURN_L = 0;
            Motor_stop(1);
        }
    }
    
    // if(usart1_ready)
    // {
    //     USART_printf(bt24_huart,HMI_arry);
    //     // UARTHMI_ParsePacket(bear_huart,HMI_arry,HMI_arry_length);
    // }
}


/**
 ****************************************************************************************************
 * 中断回调
 ****************************************************************************************************
 */


//定时器中断回调函数
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
    if(htim == &htim8)     //传输数据所用   //内部时钟72MHz, PSC == 7200-1, ARR == 500-1    //频率20Hz 每50毫秒触发一次
    {
        //定时器主延时
        if(TIMER8_FLAG<9) 
        {
            TIMER8_FLAG++;
			if(!NRF24_FLAG) NRF24_FLAG = 1;
            // RX_Dispose(RF2G4_Receive_Data); 
            
        }
        else if(TIMER8_FLAG<11)
        {
            TIMER8_FLAG++;
			if(!GNSS_FLAG) GNSS_FLAG = 1;
            // G4_GNSSDATA_Dispose(bear_huart);
            
        }
        else
        {
            TIMER8_FLAG=0;
            if(ADC_FLAG < 3) ADC_FLAG++;
            else ADC_FLAG = 0;
            // ADC_DATA_Dispose_random(bear_huart);
			system_Test_LED(0);
        }

        //自主巡航转向状态
        if(move_started && AUTO_MOVE_TURN != 0)
        {
            if(AUTO_MOVE_TURN < 100 )
            {
                AUTO_MOVE_TURN++;
            }
            else if(AUTO_MOVE_TURN < 120)
            {
                AUTO_MOVE_TURN++;            
                move_starte = 1;            
            }
            else if(AUTO_MOVE_TURN)
            {
                AUTO_MOVE_TURN = 0;
                AUTO_MOVE_TURN_L = 0;
                move_starte = 0;
            }
        }
        //间歇式运动
        if(move_started && AUTO_MOVE_TIMER != 0)
        {
            if(AUTO_MOVE_TIMER < 100)
            {
                AUTO_MOVE_TIMER++;
            }
            else if(AUTO_MOVE_TIMER < 120)
            {
                AUTO_MOVE_TIMER++;
                move_starte = 1;
            }
            else
            {
                AUTO_MOVE_TIMER = 0;
                move_starte = 0;
            } 
        }
        // USART_printf(SHOW_HUART,"move_started: %d\r\n",&move_started);
    }
    else if(htim == &htim5)     //2.4G模块所用  //内部时钟72MHz, PSC == 72-1, ARR == 2000-1     //频率500Hz 每2毫秒触发一次
    {
        // 检查是否为 TIM5 更新中断
        if(__HAL_TIM_GET_FLAG(&htim5, TIM_FLAG_UPDATE) != RESET &&__HAL_TIM_GET_IT_SOURCE(&htim5, TIM_IT_UPDATE) != RESET)
        {
            // 清除 TIM5 更新中断标志
            __HAL_TIM_CLEAR_FLAG(&htim5, TIM_FLAG_UPDATE);

            // 增加计数器
            C_2G4_Command++;

            // 判断是否超时
            if (C_2G4_Command >= 500)  // 命令有效时间 = 0.5 秒
            {
                C_2G4_Command = 0;     // 重置计数器
                F_2G4_Command_Valid = 0;  // 清除 2.4G 命令有效标志位
            }
        }
        
    }
    else  if (htim == &htim9)     //超声波测距所用 //内部时钟72MHz, PSC == 72-1, ARR == 65536-1    //频率15.26Hz 每65.536毫秒触发一次
    {
        msHcCount++; // 每次 TIM9 溢出中断，增加 1ms

    }
}

